package Robot;

import java.util.ArrayList;
import java.util.List;

import lejos.robotics.navigation.Pose;
import simulator.robot.Simulator;
import Gui.MainGui;
import Program.Execute;
import Program.LightValueListener;
import Program.SonarAngleListener;
import Program.SpeedListener;

public class RobotControl
{
	private final List<SpeedListener> speedListeners = new ArrayList<SpeedListener>();
	private final List<LightValueListener> lightValueListeners = new ArrayList<LightValueListener>();
	private final List<SonarAngleListener> sonarAngleListeners = new ArrayList<SonarAngleListener>();
	
	public int BLACK_BROWN = 387;
	public int BROWN_WHITE = 484;
	
	private MainGui gui;
	private Simulator simulator;
	
	private RobotDirection driveDirection = RobotDirection.NONE;
	private RobotDirection currentDirection = RobotDirection.NONE;
	private Pose robotPose;
	private boolean resetPosition;
	private double sensorAngle;
	private int desiredSonarAngle = 0;
	private int lightValue;
	private int sonarDistance = 255; // Default value
	private boolean touchLeft;
	private boolean touchRight;
	private final int[] pingValues = new int[8];

	private double travelSpeed = 15; // standaard travelSpeed
	private double rotateSpeed = 100; // standaard rotateSpeed
	private final int sonarRotateSpeed = 600;

	private boolean discover;
	private boolean drivePolygon;
	private int numberOfSides;
	private int lengthOfSides;
	private boolean remoteControl;
	private double travelDistance;
	private double rotateAngle;
	private boolean goToMiddle;
	private boolean whiteLine;
	private boolean playTune;
	
	private boolean isSimulator = true;
	private boolean changePilot = false;
	
	private Execute execute;
	
	public RobotControl()
	{
		setTravelSpeed(15);
		setRotateSpeed(100);
	}
	
	private void reset() {
		driveDirection = RobotDirection.NONE;
		resetPosition = false;
		desiredSonarAngle = 0;
		
		discover = false;
		drivePolygon = false;
		travelDistance = 0;
		rotateAngle = 0;
		goToMiddle = false;
		whiteLine = false;
		playTune = false;
		
		changePilot = false;
	}
	
	public void addSpeedListener(SpeedListener toAdd) {
		speedListeners.add(toAdd);
	}
	
	public void removeSpeedListener(SpeedListener teRemove) {
		speedListeners.remove(teRemove);
	}
	
	public void addLightValueListener(LightValueListener toAdd) {
		lightValueListeners.add(toAdd);
	}
	
	public void removeLightValueListener(LightValueListener teRemove) {
		lightValueListeners.remove(teRemove);
	}
	
	public void addSonarAngleListener(SonarAngleListener toAdd) {
		sonarAngleListeners.add(toAdd);
	}
	
	public void removeSonarListener(SonarAngleListener teRemove) {
		sonarAngleListeners.remove(teRemove);
	}
	
	private void notifyLightListeners() {	//TODO test
		for (LightValueListener listener: lightValueListeners)
			synchronized (listener) {
				listener.notify();
			}
	}

	public Simulator getSimulator() {
		return simulator;
	}


	public void setSimulator(Simulator simulator) {
		this.simulator = simulator;
	}
	
	public void setExecuteThread(Execute exec)
	{
		this.execute = exec;
		this.addSpeedListener(execute);
	}
	
	public void stopExecute() {
		execute.stop();
		Execute clone = execute.clone();
		this.reset();
		execute = clone;
		execute.start();
	}

	public void setMainGui(MainGui gui) {
		this.gui = gui;
	}
	

	
	/**
	 * @return the travelSpeed
	 */
	public double getTravelSpeed() {
		return travelSpeed;
	}

	/**
	 * Voor de GUI
	 * 
	 * @param travelSpeed the travelSpeed to set
	 */
	public void setTravelSpeed(double travelSpeed) {
		this.travelSpeed = travelSpeed;
		for (SpeedListener listener: speedListeners)
			listener.handleTravelSpeedChange(travelSpeed);
	}
	
	/**
	 * @return the rotateSpeed
	 */
	public double getRotateSpeed() {
		return rotateSpeed;
	}

	/**
	 * Voor de GUI
	 * 
	 * @param rotateSpeed the rotateSpeed to set
	 */
	public void setRotateSpeed(double rotateSpeed) {
		this.rotateSpeed = rotateSpeed;
		for (SpeedListener listener: speedListeners)
			listener.handleRotateSpeedChange(rotateSpeed);
	}
	
	public int getSonarRotateSpeed() {
		return sonarRotateSpeed;
	}

	/**
	 * @return the desiredSonarAngle
	 */
	public int getDesiredSonarAngle() {
		return desiredSonarAngle;
	}

	/**
	 * @param desiredSonarAngle the desiredSonarAngle to set
	 */
	public void setDesiredSonarAngle(int desiredSonarAngle) {
		this.desiredSonarAngle = desiredSonarAngle;
		gui.updateSonarArrow(desiredSonarAngle);
		for (SonarAngleListener listener: sonarAngleListeners)
			listener.handleSonarAngleChange(desiredSonarAngle);
	}
	
	private void notifyExecute()
	{
		synchronized (execute) {
			execute.notify();
		}
	}
	
	public void setRobotPose(Pose pose) {
		this.robotPose = pose;
		gui.updatePose(pose);
	}
	
	public Pose getRobotPose(Pose pose) {
		return robotPose;
	}
	
	/**
	 * @return the resetPosition
	 */
	public boolean isResetPosition() {
		return resetPosition;
	}


	/**
	 * @param resetPosition the resetPosition to set
	 */
	public void setResetPosition(boolean resetPosition) {
		this.resetPosition = resetPosition;
		notifyExecute();
	}
	
	//	Deel motoren	//

	/**
	 * @return the currentDirection
	 */
	public RobotDirection getCurrentDirection() {
		return currentDirection;
	}

	/**
	 * @param currentDirection the currentDirection to set
	 */
	public void setCurrentDirection(RobotDirection currentDirection) {
		this.currentDirection = currentDirection;
		gui.setRobotDirection(currentDirection);
	}
	
	public RobotDirection getDriveDirection() {
		return driveDirection;
	}

	/**
	 * Mehtode voor de GUI.
	 */
	public void setDriveDirection(RobotDirection direction) {
		if (! driveDirection.equals(direction)) {
			this.driveDirection = direction;
			notifyExecute();
		}
	}
	
	/**
	 * @return the travelDistance
	 */
	public double getTravelDistance() {
		return travelDistance;
	}

	/**
	 * @param travelDistance the travelDistance to set
	 */
	public void setTravelDistance(double travelDistance) {
		this.travelDistance = travelDistance;
		if(travelDistance != 0)
			notifyExecute();
	}

	/**
	 * @return the rotateAngle
	 */
	public double getRotateAngle() {
		return rotateAngle;
	}

	/**
	 * @param rotateAngle the rotateAngle to set
	 */
	public void setRotateAngle(double rotateAngle) {
		this.rotateAngle = rotateAngle;
		if(rotateAngle != 0)
			notifyExecute();
	}

	/**
	 * @return the sensorAngle
	 */
	public double getSensorAngle() {
		return sensorAngle;
	}

	/**
	 * @param sensorAngle the sensorAngle to set
	 */
	public void setSensorAngle(double sensorAngle) {
		this.sensorAngle = sensorAngle;
		gui.setSensorAngle(sensorAngle);
	}
	
	//	Deel sensoren	//
	
	/**
	 * @return the lightValue
	 */
	public int getLightValue() {

			return this.lightValue;
        
	}

	
	public void setLightValue(int value) {
		this.lightValue = value;	
		gui.setLightValue(value);
		gui.setLightColour(getLightColour());
//		notifyLightListeners();			//TODO test
	}
	
	public SensorColour getLightColour()
	{
		if(getLightValue() <= BLACK_BROWN)
			return SensorColour.BLACK;
		if(getLightValue() <= BROWN_WHITE)
			return SensorColour.BROWN;
		else
			return SensorColour.WHITE;
	}
	
	/**
	 * @return the sonarDistance
	 */
	public int getSonarDistance() {
		return sonarDistance;
	}

	/**
	 * @param sonarDistance the sonarDistance to set
	 */
	public void setSonarDistance(int sonarDistance) {
		this.sonarDistance = sonarDistance;
		gui.setSonarDistance(sonarDistance);
	}
	
	//	Deel info	//
	
	/**
	 * @return the pingValues
	 */
	public int[] getPingValues() {
		return pingValues.clone();
	}
	
	public boolean isTouchLeft(){
		return touchLeft;
	}
	
	public void setTouchLeft(boolean touchLeft){
		this.touchLeft = touchLeft;
		gui.setTouchLeft(touchLeft);
	}
	
	public boolean isTouchRight(){
		return touchRight;
	}
	
	public void setTouchRight(boolean touchRight){
		this.touchRight = touchRight;
		gui.setTouchRight(touchRight);
	}


	/**
	 * @param values the pingValues to set
	 */
	public void setPingValues(int[] values) {
		int max = Math.min(pingValues.length, values.length);
		
		for (int i = 0; i < max; i++)
			this.pingValues[i] = values[i];
		
		//TODO tekenen in de GUI
	}


	public boolean isChangePilot() {
		return changePilot;
	}
	
	public void setChangePilot(boolean change) {
		changePilot = change;
		notifyExecute();
	}
	
	public void changedPilot() {
		if (isSimulator) {
			isSimulator = false;
			gui.setPilot("NXT Robot");
		} else {
			isSimulator = true;
			gui.setPilot("Simulator");
		}
	}

	public void addAction(String text) {
		gui.addAction(text);
	}
	
	public void addDebug(String text) {
		gui.addDebug(text);
	}
	
	//	Deel acties	//
	
	/**
	 * @return the whiteLine
	 */
	public boolean isWhiteLine() {
		return whiteLine;
	}

	/**
	 * @param whiteLine the whiteLine to set
	 */
	public void setWhiteLine(boolean whiteLine) {
		this.whiteLine = whiteLine;
		notifyExecute();
	}

	/**
	 * @return the playTune
	 */
	public boolean isPlayTune() {
		return playTune;
	}

	/**
	 * @param playTune the playTune to set
	 */
	public void setPlayTune(boolean playTune) {
		this.playTune = playTune;
		notifyExecute();
	}

	/**
	 * @return the discover
	 */
	public boolean isDiscover() {
		return discover;
	}


	/**
	 * @param discover the discover to set
	 */
	public void setDiscover(boolean discover) {
		this.discover = discover;
		if (discover)
			notifyExecute();
	}


	/**
	 * @return the remoteControl
	 */
	public boolean isRemoteControl() {
		return remoteControl;
	}

	/**
	 * Voor de GUI
	 * 
	 * @param remoteControl the remoteControl to set
	 */
	public void setRemoteControl(boolean remoteControl) {
		this.remoteControl = remoteControl;
	}
	
	/**
	 * @return the goToMiddle
	 */
	public boolean isGoToMiddle() {
		return goToMiddle;
	}


	/**
	 * @param goToMiddle the goToMiddle to set
	 */
	public void setGoToMiddle(boolean goToMiddle) {
		this.goToMiddle = goToMiddle;
		notifyExecute();
	}
	
	private boolean colorCalibration = false;
	public void setColorCalibration(boolean val)
	{
		this.colorCalibration = val;
		this.notifyExecute();
	}
	
	public boolean isColorCalibration()
	{
		return this.colorCalibration;
	}
	
	private SensorColour colorToCalibrate = SensorColour.NONE;
	public void setColorToCalibrate(SensorColour color)
	{
		this.colorToCalibrate = color;
	}
	
	public SensorColour getColorToCalibrate()
	{
		return this.colorToCalibrate;
	}


	/**
	 * Voor de GUI
	 * 
	 * @param execute
	 */
	public void setDrivePolygon(boolean execute) {
		drivePolygon = execute;
		this.notifyExecute();
	}
	
	/**
	 * Voor de GUI en de Execute
	 * 
	 * @param numberOfSides
	 * @param sizeSides
	 */
	public void setPolygon(int numberOfSides, int sizeSides) {
		this.numberOfSides = numberOfSides;
		this.lengthOfSides = sizeSides;
	}
	
	public int getLengthOfSides() {
		return lengthOfSides;
	}
	
	public int getNumberOfSides() {
		return numberOfSides;
	}
	
	public boolean drivePolygon() {
		return drivePolygon;
	}

	public boolean getPolygonTurnDirection() {
		return polygonTurnDirection;
	}

	private boolean polygonTurnDirection = true;
	public void setPolygonTurnDirection(boolean right)
	{
		this.polygonTurnDirection = right;
	}
}
